package org.example.util;


import org.example.bean.Cord;
import org.example.bean.GameMap;
import org.example.bean.JsonGamePos;
import org.example.bean.LazyNode;

import java.util.*;

/**
 * @Desc:
 * @Author: yanghongjun
 * @Date: 2024/5/17
 **/
public class LazyThetaStarUtil {
    private static final List<int[]> DIRECTIONS = Arrays.asList(
            new int[]{0, 1}, new int[]{1, 1}, new int[]{1, 0},
            new int[]{1, -1}, new int[]{0, -1}, new int[]{-1, -1},
            new int[]{-1, 0}, new int[]{-1, 1}
    );

    public static List<JsonGamePos> findPath(LazyNode start, LazyNode goal, GameMap gameMap) {
        PriorityQueue<LazyNode> openList = new PriorityQueue<>(Comparator.comparingDouble(LazyNode::getfScore));
        Set<String> closedSet = new HashSet<>();

//        start.gScore = 0;
//        start.hScore = heuristic(start, goal);
//        start.fScore = start.gScore + start.hScore;

        start.setgScore(0);
        start.sethScore(heuristic(start, goal));
        start.setfScore(start.getgScore() + start.gethScore());
        openList.add(start);

        while (!openList.isEmpty()) {
            LazyNode currentNode = openList.poll();

            if (currentNode.getX() == goal.getX() && currentNode.getY() == goal.getY()) {
                List<Cord> lazyNodes = reconstructPath(currentNode);
                long l = System.currentTimeMillis();
                List<JsonGamePos> lazyNodes1 = gameMap.optimizePath(lazyNodes);
                System.out.println("LazyThetaStarUtil 平滑路径耗时：" + (System.currentTimeMillis() - l) + "平滑前节点：" + lazyNodes.size() +
                        "平滑后节点："+ lazyNodes1.size());
                return lazyNodes1;
            }

            closedSet.add(currentNode.getX() + "," + currentNode.getY());

            for (int[] direction : DIRECTIONS) {
                int newX = currentNode.getX() + direction[0];
                int newY = currentNode.getY() + direction[1];

                if (!gameMap.isValid(newX, newY) || closedSet.contains(newX + "," + newY)) {
                    continue;
                }

                LazyNode neighbor = new LazyNode(newX, newY);
                if (!lineOfSight(currentNode, neighbor, gameMap.getMaps())) {
                    continue;
                }
                //neighbor.setgScore(currentNode.getgScore() + distance(currentNode, neighbor));

                neighbor.sethScore(heuristic(neighbor, goal));
                neighbor.setfScore(neighbor.getgScore() + neighbor.gethScore());
                neighbor.setParent(currentNode);
                if (!openList.contains(neighbor) || neighbor.getgScore() < openList.stream()
                        .filter(node -> node.getX() == neighbor.getX() && node.getY() == neighbor.getY())
                        .findFirst()
                        .orElseThrow(() -> new NoSuchElementException("Node not found in open list"))
                        .getgScore()) {
                    openList.add(neighbor);

                }
            }
        }

        return Collections.emptyList(); // No path found
    }

    private static boolean lineOfSight(LazyNode from, LazyNode to, int[][] grid) {
        int dx = to.getX() - from.getX();
        int dy = to.getY() - from.getY();

        // Check for horizontal, vertical, or diagonal movement
        int steps = Math.abs(dx) > Math.abs(dy) ? Math.abs(dx) : Math.abs(dy);
        double xIncrement = dx / (double) steps;
        double yIncrement = dy / (double) steps;

        double currentX = from.getX();
        double currentY = from.getY();

        for (int step = 0; step < steps; step++) {
            currentX += xIncrement;
            currentY += yIncrement;

            int roundedX = (int) Math.round(currentX);
            int roundedY = (int) Math.round(currentY);

            if (grid[roundedX][roundedY] == 1) {
                return false; // Obstacle found
            }
        }

        return true;
    }


    /**
     * 根据给定的当前节点，重建并返回从起点到该节点的路径。
     *
     * @param currentNode 当前节点
     * @return 从起点到当前节点的路径
     */
    private static List<Cord> reconstructPath(LazyNode currentNode) {
        List<Cord> path = new ArrayList<>();
        while (currentNode != null) {
            path.add(new Cord(currentNode.getX(), currentNode.getY()));
            currentNode = currentNode.getParent();
        }
        Collections.reverse(path);
        return path;
    }

    private static double heuristic(LazyNode a, LazyNode b) {
        // Using Euclidean distance as heuristic
        return Math.sqrt(Math.pow(a.getX() - b.getX(), 2) + Math.pow(a.getY() - b.getY(), 2));
    }

    private static double distance(LazyNode a, LazyNode b) {
        // Using Euclidean distance
        return heuristic(a, b);
    }

    private static boolean isValid(int x, int y, int[][] grid) {
        return x >= 0 && x < grid.length && y >= 0 && y < grid[0].length && grid[x][y] != 1;
    }
}
